
// #include "behaviortree_cpp/action_node.h"
// #include "behaviortree_cpp/condition_node.h"
#include <chrono>
#include "utils/load_data.hpp"
// #include "sensor_data.hpp"
#include <iterator>  // std::distance
#include <algorithm> // std::max_element
#include <vector>
#include <deque>
#include <numeric>
#include <stdexcept>
// ANSI 颜色代码
#define COLOR_RED     "\033[31m"
#define COLOR_GREEN   "\033[32m"
#define COLOR_YELLOW  "\033[33m"
#define COLOR_BLUE    "\033[34m"
#define COLOR_MAGENTA "\033[35m"
#define COLOR_CYAN    "\033[36m"
#define COLOR_RESET   "\033[0m"    
using namespace std::chrono_literals;
enum State
{
    kHSDetectionState, 
    kTODetectionState
};